/**
 * @file DijkstraPlanner.h
 * @brief Dijkstra算法路径规划器
 * @details
 * Dijkstra算法原理：
 * 1. 从起点开始，初始化所有节点的距离为无穷大，起点距离为0
 * 2. 将起点加入优先队列，按距离排序
 * 3. 每次从队列中取出距离最小的节点
 * 4. 遍历该节点的所有邻居，如果找到更短的路径则更新距离
 * 5. 重复直到到达目标点或队列为空
 *
 * 特点：保证找到最短路径，但搜索效率较低
 */

#ifndef DIJKSTRA_PLANNER_H
#define DIJKSTRA_PLANNER_H

#include "core/PlannerBase.h"
#include <queue>
#include <unordered_map>

namespace PathPlanning
{
/**
 * @brief Dijkstra算法配置参数
 */
struct DijkstraConfig : public PlannerConfig
{
    bool   eightConnected    = true;  // 使用8方向连接还是4方向
    double obstacleThreshold = 128.0; // 障碍物阈值

    DijkstraConfig()
    {
        plannerName = "Dijkstra";
    }
};

/**
 * @brief Dijkstra搜索节点
 */
struct DijkstraNode : public Node
{
    GridCell gridCell;

    DijkstraNode(const GridCell& cell, const Point& worldPos) : gridCell(cell)
    {
        position = worldPos;
    }
};

/**
 * @brief 节点比较器 (用于优先队列)
 */
struct DijkstraNodeCompare
{
    bool operator()(const std::shared_ptr<DijkstraNode>& a, const std::shared_ptr<DijkstraNode>& b) const
    {
        return a->gCost > b->gCost; // 最小堆
    }
};

class DijkstraPlanner : public PlannerBase
{
public:
    DijkstraPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    /**
     * @brief 从网格坐标重建路径
     */
    std::vector<Pose>
    reconstructPath(const std::unordered_map<GridCell, std::shared_ptr<DijkstraNode>, GridCellHash>& cameFrom,
                    const std::shared_ptr<DijkstraNode>&                                             endNode);
};

} // namespace PathPlanning

#endif